﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PIDControl
{
    public class PIDController
    {
        private double _kp; // 比例增益
        private double _ki; // 积分增益
        private double _kd; // 微分增益

        private double _setpoint; // 设定值
        private double _prevError; // 上一次的误差
        private double _integral; // 积分累积

        private double _outputMin; // 输出最小值
        private double _outputMax; // 输出最大值

        public PIDController(double kp, double ki, double kd, double setpoint, 
            double outputMin, double outputMax)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _setpoint = setpoint;
            _outputMin = outputMin;
            _outputMax = outputMax;

            _prevError = 0;
            _integral = 0;
        }

        public double Compute(double input, double dt)
        {
            // 计算误差
            double error = _setpoint - input;

            // 计算比例项
            double proportional = _kp * error;

            // 计算积分项
            _integral += error * dt;
            double integral = _ki * _integral;

            // 计算微分项
            double derivative = _kd * (error - _prevError) / dt;

            // 计算输出
            double output = proportional + integral + derivative;

            // 限制输出范围
            output = Clamp(output, _outputMin, _outputMax);

            // 保存当前误差
            _prevError = error;

            return output;
        }

        // 手动实现 Clamp 方法
        private double Clamp(double value, double min, double max)
        {
            if (value < min)
                return min;
            if (value > max)
                return max;
            return value;
        }
        public void SetSetpoint(double setpoint)
        {
            _setpoint = setpoint;
        }

        public void Reset()
        {
            _prevError = 0;
            _integral = 0;
        }
    }
}
